#include "../libHardware/Transport/SerialPort/include/SerialPort.hpp"
#include "../libBase/include/common.h"
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <queue>
#include <thread>
#include <mutex>

#include <fcntl.h>
#include <unistd.h>
#include "../libHardware/motorcontrol/include/motorcontrol.hpp"
#include <boost/asio.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>

/**
 * 使用时加sudo
 * .
 * 发送地址01
 * 返回命令
 * 如果成功返回 01 02 6B 如果失败01 EE 6B
 * 编码器数值 01 uint16_t 6B := (uint16_t)(((uint16_t)0x23 << 8) |((uint16_t)0x15 << 0))
 * 电机位置 01 int32_t 6B:(int32_t)(((int32_t)0x56 << 24) |((int32_t)0x23 << 16) |((int32_t)0x75 << 8) |((int32_t)0x04 << 0))
 * 电机转动角度为 电机位置*360/65536
 * 位置误差 01 int16_t 6B:= (uint16_t)(((uint16_t)0x23 << 8) |((uint16_t)0x15 << 0))
 * 使能状态 01 00 6B false 01 01 6B true
 * 堵转状态 01 00 6B 未堵转 01 01 6B true
 * 单圈上电自动回零状态标志01 00 6B 正常回零 01 01 6B 出现异常，上电回零错误
 * 操纵命令
 * 触发校准 01 06 45 6B
 * 设置当前位置为零点 01 0A 6D 6B
 * 解除堵转 01 0E 52 6B
 *
 *
 * 读取编码器值 01 30 6B
 * 读取输入脉冲
 * 读取电机实时位置 01 36 6B
 * 读取位置误差 01 39 6B
 * 读取堵转状态 01 3E 6B
 * 读取单圈上电自动回零状态标志 01 3F 6B
 *
 *
 * 修改当前细分步数 01 84 细分步数 6B
 *
 *
 * 速度控制
 * 读取使能状态 01 3A 6B
 * 控制电机不/使能 01 F3 00/01 6B
 * 控制闭环电机的正反转,速度 01 F6 14 FF 00 6B :方向为逆时针转1，速度为4FF（0-1279），加速度为00（加速曲线档位）FF（255不启用曲线加减速）
 * 缓慢/立即停止 01 F6 10 00 00/FF 6B//
 * 清除当前速度模式 01 FF CA 6B//
 * 存储当前速度模式 01 FF C8 6B
 * 位置控制
 * 控制闭环电机的正反转,速度 01 FD 14 FF 00 ，00 0C 80 6B    16 运动 0x000C80 个脉冲，即 3200 个脉冲，让 1.8°的电机转动一圈，一圈就是4mm
 * 闭环电机收到指令，会先返回 01 02 6B 的命令进行应答,；紧接着，闭环电机内部更新完全部的脉冲（位置），会返回 01 9F 6B 的命令(无返回PcmdRet 选项设置为 Disable)
 *
 *
 */
// 一个int8，8个二进制数字，一个16位数字等于2个二进制数字

using namespace wmj;

int main(int argc, char **argv)
{
    std::shared_ptr<wmj::SerialPort> serial_port = std::make_shared<wmj::SerialPort>();
    wmj::Rate send_rate(200);
    int count = 0;
    timeval tv;
    gettimeofday(&tv, NULL);
    long time_now = tv.tv_sec * 1000000 + tv.tv_usec;
    wmj::Buffer data_to_send{};
    // cv::Mat empty(1024, 1280, CV_8UC3, cv::Scalar::all(0));s
    bool IfSend = true;
    char c = 'h';
    char Lastc = c;
    std::thread(wmj::monitorKeyboard, &c).detach();
    bool Quitprogram = false;
    int DirFlag = 1;
    bool MoveConstant = false;
    double lasttime = wmj::now();
    int DataLength = 8;
    int AskPosi = 1;
    while (true)
    {
        if (Lastc != c)
        {
            switch (c)
            {
            case 's':
                wmj::VelocityControl(1, 1, 02, data_to_send, DataLength); // 向上
                MoveConstant = false;
                IfSend = false;
                break;
            case 'a':
                wmj::VelocityControl(0, 1, 02, data_to_send, DataLength);
                IfSend = false;
                MoveConstant = false;
                break;
            case 'q':
                wmj::ClearControl(data_to_send, DataLength);
                IfSend = false;
                MoveConstant = false;
                Quitprogram = true;
                break;
            case 'd':
                wmj::ClearControl(data_to_send, DataLength);
                IfSend = false;
                MoveConstant = false;
                break;
            case 'w':
                wmj::RotateEnable(data_to_send, DataLength);
                IfSend = false;
                MoveConstant = false;
                break;
            case 'f':
                MoveConstant = true;
                break;
            case 'z':
                wmj::askForinfo(data_to_send, DataLength);
                IfSend = false;
                MoveConstant = false;
                break;
            case 'e':
                wmj::StopControl(data_to_send, 1, DataLength);
                IfSend = false;
                MoveConstant = false;
                break;
            }
            Lastc = c;
        }
        if (MoveConstant)
        {
            if (abs(wmj::now() - lasttime) > 2)
            {
                DirFlag = DirFlag + 1;
                if ((DirFlag) % 2 == 1)
                {
                    wmj::VelocityControl((DirFlag) % 2, 1, 1, data_to_send, DataLength);
                }
                if ((DirFlag) % 2 == 0)
                {
                    wmj::VelocityControl((DirFlag) % 2, 1, 1, data_to_send, DataLength);
                }
                lasttime = wmj::now();
                IfSend = false;
            }
        }
        if (AskPosi % 2 == 1)
        {
            if (IfSend == false)
            {
                int length = checkLength(data_to_send);
                std::cout << "data_to_send:";
                for (int i = 0; i < length; i++)
                    printf("\t%#X ", data_to_send[i]);
                std::cout << std::endl;
                // std::cout << DataLength<< std::endl;
                boost::asio::write(*serial_port->m_sp_send_port, boost::asio::buffer(data_to_send, length));
                // serial_port->sendFrame(data_to_send);
                data_to_send.clear();
                IfSend = true;
            }
        }
        else
        {
            wmj::Buffer tempData;
            wmj::askForinfo(tempData, DataLength);
            boost::asio::write(*serial_port->m_sp_send_port, boost::asio::buffer(tempData, 3));
        }
        AskPosi += 1;
        if (Quitprogram == true)
        {
            usleep(100);
            return 0;
        }
        send_rate.sleep();
        count++;
    }
    return 0;
}
